package com.gitee.wsl.mathematics.matrix.mat4.ext

import com.gitee.wsl.mathematics.complex.quaternion.Quaternion
import com.gitee.wsl.mathematics.matrix.mat4.MutableMatrix4
import com.gitee.wsl.mathematics.vector.vec3.Vector3
import com.gitee.wsl.mathematics.vector.vec4.Vector4


/**
 * Sets the matrix to a rotation matrix representing the quaternion.
 *
 * @param quaternion the quaternion that is to be used to set this matrix
 * @return this matrix
 */
fun<T:Number> MutableMatrix4<T, *, *>.setComponent(quaternion: Quaternion<T, *>) = setComponent(quaternion.x, quaternion.y, quaternion.z, quaternion.w)


/**
 * Sets the matrix to a rotation matrix representing the quaternion.
 *
 * @param qx The X component of the quaternion that is to be used to set this matrix.
 * @param qy The Y component of the quaternion that is to be used to set this matrix.
 * @param qz The Z component of the quaternion that is to be used to set this matrix.
 * @param qw The W component of the quaternion that is to be used to set this matrix.
 * @return this matrix
 */
fun<T:Number> MutableMatrix4<T, *, *>.setComponent(qx: T, qy: T, qz: T, qw: T) = setComponent(zero, zero, zero, qx, qy, qz, qw)

/**
 * Sets the matrix to a rotation matrix representing the translation, quaternion, and scale.
 *
 * @param translation the translation component
 * @param quaternion the rotation component
 * @param scale the scale component
 * @return this matrix
 */
fun<T:Number> MutableMatrix4<T, *, *>.setComponent(translation: Vector3<T, *>, quaternion: Vector4<T, *>, scale: Vector3<T, *>) =
    setComponent(
        translation.x, translation.y, translation.z,
        quaternion.x, quaternion.y, quaternion.z, quaternion.w,
        scale.x, scale.y, scale.z
    )

/**
 * Sets the matrix to a rotation matrix representing the translation, quaternion, and scale.
 *
 * @param tx The X component of the translation that is to be used to set this matrix.
 * @param ty The Y component of the translation that is to be used to set this matrix.
 * @param tz The Z component of the translation that is to be used to set this matrix.
 * @param qx The X component of the quaternion that is to be used to set this matrix.
 * @param qy The Y component of the quaternion that is to be used to set this matrix.
 * @param qz The Z component of the quaternion that is to be used to set this matrix.
 * @param qw The W component of the quaternion that is to be used to set this matrix.
 * @param sx The X component of the scaling that is to be used to set this matrix.
 * @param sy The Y component of the scaling that is to be used to set this matrix.
 * @param sz The Z component of the scaling that is to be used to set this matrix.
 * @return this matrix
 */
fun<T:Number> MutableMatrix4<T, *, *>.setComponent(
    tx: T, ty: T, tz: T,
    qx: T, qy: T, qz: T, qw: T,
    sx: T = one, sy: T = one, sz: T = one,
) {
    var s = sqrt(qw * qw + qx * qx + qy * qy + qz * qz)
    s = 1f / (s * s)

    m00 = sx * (1 - 2 * s * (qy * qy + qz * qz))
    m01 = sy * (2 * s * (qx * qy - qz * qw))
    m02 = sz * (2 * s * (qx * qz + qy * qw))
    m03 = tx

    m10 = sx * (2 * s * (qx * qy + qz * qw))
    m11 = sy * (1 - 2 * s * (qx * qx + qz * qz))
    m12 = sz * (2 * s * (qy * qz - qx * qw))
    m13 = ty

    m20 = sx * (2 * s * (qx * qz - qy * qw))
    m21 = sy * (2 * s * (qy * qz + qx * qw))
    m22 = sz * (1 - 2 * s * (qx * qx + qy * qy))
    m23 = tz

    m30 = zero
    m31 = zero
    m32 = zero
    m33 = one

}
